#coding=utf-8
'''
    显示机器人足底和身体姿态的模块
'''

import pyqtgraph.opengl as gl
import numpy as np
import threading
from basic_math.gyro_transform import Trans3D
class Gyro3DWidget(gl.GLViewWidget):
    '''
        用于显示3D环境下的姿态以及四点力反馈矩阵
    '''
    def __init__(self,plateSize=[0.07,0.044],parent=None):
        '''
        :param plateSize:
        '''
        super().__init__()
        '''
            配置初始参数
        '''
        self.setCameraPosition(distance=0.5)
        self.setBackgroundColor('#aaa')
        #self.show()
        self.parent=parent
        #   旋转参数
        self.roll=0.0;self.pitch=0.0;self.yaw=0.0
        #   平移参数
        self.trans=np.zeros((3,1))
        #   内部数据
        self.plateSize=plateSize    #显示界面中的足底平面大小
        x=plateSize[0]/2;y=plateSize[1]/2
        #   构建Gyroplate矩阵，显示为足底平面
        #   3X4矩阵，每一列是一个点
        self.gyroPlate=np.array([[-x,y,0.0],[x,y,0.0],[x,-y,0.0],[-x,-y,0.0]]).transpose()
        #   压力参数，3X4矩阵，每一列是一个传感器感知信息，当前是一维压力，所以只有第三行是有数据的
        self.pressureMatrix=np.zeros((3,4))
        #   显示缓存(足底)
        self.vertexs=Trans3D(self.gyroPlate)
        self.faces= np.array([[0, 1, 3],[1, 2, 3]],dtype=np.uint8)
        self.faceColors = np.array([
            [0, 0, 1, 1.0],
            [0, 0, 1, 1.0]
        ])


        #   显示缓存（压力平面）
        self.pColors = np.array([
            [0.5, 0.5, 0.5, 1.0],
            [0.5, 0.5, 0.5, 1.0]
        ])

        #   更新内容以防无平面报错

        #   添加进View
        self.plateLines = gl.GLLinePlotItem()
        self.pressLines = gl.GLLinePlotItem()
        self.UpdateView()
        self.addItem(self.plateLines)
        self.addItem(self.pressLines)

        self.addItem(gl.GLGridItem())



    def SetGyro(self,r=0,p=0,y=0):
        '''
        设置姿态
        :param r:
        :param p:
        :param y:
        :return:
        '''
        self.roll=r;self.pitch=p;self.yaw=y

    def SetPressure(self,matrix=np.zeros(4),axis=2):
        '''
        设置压力矩阵
        :param matrix:  矩阵
        :param axis: 轴（默认为2，代表z轴
        :return:
        '''
        matrix=np.array(matrix)
        self.pressureMatrix[axis]=matrix
    def UpdateView(self):
        '''
        更新显示
        :return:
        '''
        forceEnds = self.gyroPlate+self.pressureMatrix
        #   旋转，返回3行4列矩阵,构造新的足底平面
        vertexs=Trans3D(self.gyroPlate,self.roll,self.pitch,self.yaw).transpose()
        #   构造新的传感器压力末端点
        forceEnds=Trans3D(forceEnds,self.roll,self.pitch,self.yaw).transpose()
        #   足底平面
        self.plateLines.setData(pos=vertexs,color=(0,1,1,0.5),width=4,antialias=True)
        #   压力平面
        self.pressLines.setData(pos=forceEnds,color=(1,0,0,0.5),width=4,antialias=True)

if __name__=='__main__':
    g=Gyro3DWidget()
